#ifndef MODEL_H
#define MODEL_H
// Std
#include <iostream>
#include <math.h>
// Qt
#include <QString>
#include <QFile>
#include <QTextStream>
// PCL
#include <pcl/common/common.h>
#include <pcl/common/centroid.h>
#include <pcl/common/angles.h>
#include <pcl/common/transforms.h>
#include <pcl/console/parse.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/sample_consensus/sac_model_line.h>
#include <pcl/sample_consensus/sac_model_circle.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/visualization/pcl_visualizer.h>
// Project
#include "registration.h"

using namespace std;
using namespace pcl;

/*!
 \brief

*/
class model
{
public:
/*!
 \brief

*/
    model();
    /*!
     \brief

     \return bool
    */
    bool loadTargetCloud(); // loads target form resources, returns true if file was loaded correctly.
    /*!
     \brief

     \param path
     \return bool
    */
    bool loadSourceCloud(QString path); // returns true if file was loaded correctly.
    // GETTERS
    /*!
     \brief

     \return PointCloud<pcl::PointXYZRGB>
    */
    PointCloud<pcl::PointXYZRGB>::Ptr getTargetCloud();
    /*!
     \brief

     \return PointCloud<pcl::PointXYZRGB>
    */
    PointCloud<pcl::PointXYZRGB>::Ptr getSourceCloud();
    /*!
     \brief

     \return PointCloud<pcl::PointXYZRGB>
    */
    PointCloud<pcl::PointXYZRGB>::Ptr getRegisteredCloud();
    // FUNCTIONS
//    void registerWithLines(PointCloud<PointXYZRGB>::Ptr source, PointCloud<PointXYZRGB>::Ptr target, PointCloud<PointXYZRGB>::Ptr registered, bool verbose);
    /*!
     \brief

     \param source
     \param target
     \param min
     \param max
     \param minPercentage
     \param maxScore
     \param verbose
     \return bool
    */
    bool checkBase(PointCloud<PointXYZRGB>::Ptr source, PointCloud<PointXYZRGB>::Ptr target, Eigen::Vector4f min, Eigen::Vector4f max, int minPercentage, float maxScore, bool verbose);
    /*!
     \brief

     \param source
     \param target
     \param min
     \param max
     \param minPercentage
     \param maxColorDiff
     \param verbose
     \return bool
    */
    bool checkSide(PointCloud<PointXYZRGB>::Ptr source, PointCloud<PointXYZRGB>::Ptr target, Eigen::Vector4f min, Eigen::Vector4f max, int minPercentage, int maxColorDiff, bool verbose);

private:
    /*!
     \brief

     \param cloud_in
     \param cloud_out
     \param threshold
     \param verbose
    */
    void removePlane(PointCloud<PointXYZRGB>::Ptr cloud_in,PointCloud<PointXYZRGB>::Ptr cloud_out, double threshold, bool verbose);
    /*!
     \brief

     \param cloud
     \param indices
     \param min_size
     \param max_size
     \param tolerance
     \param verbose
     \return vector<PointIndices>
    */
    vector<PointIndices> clusterize(PointCloud<PointXYZRGB>::Ptr cloud, PointIndicesPtr indices, int min_size, int max_size, double tolerance, bool verbose);
    /*!
     \brief

     \param cloud
     \param min_size
     \param max_size
     \param tolerance
     \param verbose
     \return vector<PointIndices>
    */
    vector<PointIndices> clusterize(PointCloud<PointXYZRGB>::Ptr cloud, int min_size, int max_size, double tolerance, bool verbose);
    /*!
     \brief

     \param cloud
     \param cut_indexes
     \param min_x
     \param max_x
     \param min_y
     \param max_y
     \param min_z
     \param max_z
     \param verbose
    */
    void cutVolume(PointCloud<PointXYZRGB>::Ptr cloud, PointIndicesPtr cut_indexes, float min_x, float max_x, float min_y, float max_y, float min_z, float max_z, bool verbose);
    /*!
     \brief

     \param cloud
     \param clusters
     \param indices_lines
     \param verbose
     \return Eigen::Vector3f
    */
    Eigen::Vector3f computeLinesCloud(PointCloud<PointXYZRGB>::Ptr cloud, vector<PointIndices>* clusters, PointIndicesPtr indices_lines, bool verbose);
    /*!
     \brief

     \param cloud_in
     \param clusters
     \param indices_out
     \param verbose
    */
    void computeCentralCloud(PointCloud<PointXYZRGB>::Ptr cloud_in, vector<PointIndices>* clusters, PointIndicesPtr indices_out,bool verbose);
    /*!
     \brief

     \param source
     \param target
     \return float
    */
    float validationScoreProximityTarget (PointCloud<PointXYZRGB>::Ptr source, PointCloud<PointXYZRGB>::Ptr target);
//    Eigen::Matrix4f finalTransformation (PointCloud<PointXYZRGB>::Ptr source, PointCloud<PointXYZRGB>::Ptr target, bool verbose);
    /*!
     \brief

     \param cloud
     \param indices
     \return Eigen::Vector3i
    */
    Eigen::Vector3i getAverageColor(PointCloud<PointXYZRGB>::Ptr cloud, vector<int>* indices);
    /*!
     \brief

     \param cloud
     \return Eigen::Vector4f
    */
    Eigen::Vector4f getPlaneCoefficients(PointCloud<PointXYZRGB>::Ptr cloud);
    /*!
     \brief

     \param cloud
     \param indices
     \return float
    */
    float getAverageZ(PointCloud<PointXYZRGB>::Ptr cloud, PointIndices indices);
    /*!
     \brief

     \param source
     \param target
     \return Eigen::Matrix4f
    */
    Eigen::Matrix4f getTransformation(PointCloud<PointXYZRGB>::Ptr source, PointCloud<PointXYZRGB>::Ptr target);

    // VARIABLES
    PointCloud<PointXYZRGB>::Ptr _sourceCloud; /*!< TODO */
    PointCloud<PointXYZRGB>::Ptr _targetCloud; /*!< TODO */
    PointCloud<PointXYZRGB>::Ptr _registeredCloud; /*!< TODO */
};

#endif // MODEL_H
